#include <iostream>
#include <pthread.h>

#include "Sys3D.h"
#include "Quaternion.h"
#include "Rotation.h"
#include "MatrixClasses.h"
#include "FastOps.h"
#include "Cholesky.h"

#include "Timer.h"

using namespace RSIM;

int main (int argc, const char * argv[])
{        
        const double& q = 1.0;
        const double& u = 1.0;
        
        const int& nbodies = 256;
        const int& nproc = 1;
        const int& niter = 10000;                
        
        RSIM::Sys3D mbs3D(nbodies,nproc);                        
        mbs3D.initDCANodesForDCAABA();
        mbs3D.setQ(q);
        mbs3D.setU(u);        
        mbs3D.updRotationMatrix();
        mbs3D.updAngularVelocity();
//         mbs3D.calcJ();
        
        RSIM::Timer t;
        for(int i=0;i<niter;++i){
               mbs3D.n_formInboardEOM(0,nbodies-1);
               mbs3D.n_calcDCAHandleEquationsUsingABA_Opt(0,nbodies-1,0,true,true);
//                mbs3D.solveSystemHandleEquationsCase_OpControl();

                //mbs3D.disassembleABAAssembly_Opt(0,true,true);
        }        
//         mbs3D.Asm_[0].dca_.displayH1();
//         mbs3D.Asm_[0].dca_.displayH2();
//         cout<<mbs3D.iLambda_<<endl;
//         cout<<mbs3D.pe_<<endl;
        
        t.stop();                  
        t.printElapsedTime(0);
//         mbs3D.calcNullSpaceProjectionMatrix();
//         cout<<mbs3D.pinvjac_<<endl;        
//         cout<<mbs3D.Lambda_<<endl;
//         cout<<mbs3D.iLambda_<<endl;
//         cout<<mbs3D.Lambda_<<endl<<endl;
//         cout<<mbs3D.pe_<<endl;
//         cout<<mbs3D.Jac_<<endl;
//         Mat6x6 mat;
//         FastOps::form_rigid_body_transformation_matrix(mbs3D.barray_[nbodies-1].nrk_.Ptr(),mat.wPtr());
//         cout<<mat<<endl<<endl;
//         cout<<mbs3D.barray_[nbodies-1].nbetak_<<endl;
//         cout<<(~mat)*mbs3D.Jac_*mbs3D.pinvjac_<<endl;
//         cout<<mbs3D.null_<<endl;
        
//         mbs3D.displayvin();                
//         mbs3D.displayPosition();
//         mbs3D.displayJac();
//         mbs3D.displayTau();
//         mbs3D.displayAngularVelocity();
//         cout<<mbs3D.barray_[0].v_in_<<endl;        
//         RSIM::Vect3 tmp(0.0);
//         for(int i=0;i<nbodies-1;++i)
//                 tmp = tmp + mbs3D.barray_[i].nbetak_;
//         cout<<tmp<<endl;
//         mbs3D.displayLinearVelocity();
        
        /*
        RSIM::Timer t;
        for(int i=0;i<niter;i++){
                mbs3D.n_formInboardEOM();
                mbs3D.n_triangularizeBackward(0,nbodies-1);
                mbs3D.n_substituteForward(0,nbodies-1,NULL);
                
        }    
        t.stop();
        t.printElapsedTime(0);*/
        
//         cout<<setprecision(16);
//         for(int i=0;i<nbodies;++i)
//                 cout<<mbs3D.barray_[i].v_<<endl;
//         mbs3D.displayUDot();      
        
        return 0;
}
